#include "WPILib.h"
#include "Robot2489.h"


bool kicking = false;

	/**
	 * Runs the motors under driver control with either tank or arcade steering 
	 * selected by a jumper in DS Digin 0. 
	 * 
	 * Starts the compressor.
	 * 
	 * Enables the Watchdog.
	 * 
	 * Initializes Kicker and Latch to Relaxed and Closed respectively.
	 * 
	 * Then starts the big Operator Control while loop:
	 * 		Drives the robot using Tank Drive and the joysticks connected to ports
	 * 		1 and 2. 
	 */

	void Robot2489::OperatorControl(void)
	{
		bool strongKicking = false;
		bool mediumKicking = false;
		//bool weakKicking = false;
		bool lockout = false;
		bool strongButton; //the left stick's trigger's state, pressed or not
		//bool mediumButton; //the left stick's button 4's state
		//bool weakButton; //the left stick's button 2's state
		bool lastStrong = false;
		//bool lastMedium = false;
		//bool lastWeak = false;
		bool kickerReadying = true;
		bool latchOpened = false;
		double timerState;		
		bool towerRaiseButton = false;
		bool towerRetractButton = true;
		
		AxisCamera &camera = AxisCamera::GetInstance();
		camera.WriteResolution(AxisCamera::kResolution_320x240);
		camera.WriteCompression(20);
		camera.WriteBrightness(0);
		
		KarlKompressor->Start();
		GetWatchdog().SetEnabled(true);
		CloseLatch();
		ChargeKicker();
		while (IsOperatorControl())
		{
			//myRobot->TankDrive(leftStick, rightStick);
			myRobot->SetLeftRightMotorSpeeds(MotorControl(leftStick->GetY()), MotorControl(rightStick->GetY()));
			
			towerRaiseButton = manipStick->GetRawButton(6);
			if (towerRaiseButton)
			{
				RaiseTower();
			}
			towerRetractButton = manipStick->GetRawButton(7);
			if (towerRetractButton)
			{
				RetractTower();
			}
			if (!strongKicking && !mediumKicking)
			{
				lockout = false;
			}

			strongButton = manipStick->GetTrigger();
			if (strongButton && (lastStrong == false) && !lockout) 
			{
				strongKicking = true;
				kickTimar->Start();
				kicking = true;
				lockout = true;
			}
			lastStrong = strongButton;
			
			timerState = kickTimar->Get();
			if (strongKicking)
			{
				if (timerState < LATCHOPENTIME && !latchOpened)
				{
					OpenLatch();
					latchOpened = true;
				}
				if (timerState >= LATCHOPENTIME && kickerReadying)
				{
					RetractKicker();
					kickerReadying = false;
				}
				if (timerState >= (LATCHOPENTIME + KICKERRETRACTTIME) && latchOpened)
				{
					CloseLatch();
					latchOpened = false;
				}
				if (timerState >= (LATCHOPENTIME + KICKERRETRACTTIME + LATCHCLOSETIME) && !kickerReadying)
				{
					ChargeKicker();
					kickerReadying = true;
				}
				if (timerState >= (LATCHOPENTIME + KICKERRETRACTTIME + LATCHCLOSETIME + KICKERREADYINGTIME))
				{
					kickTimar->Stop();
					kickTimar->Reset();
					strongKicking = false;
				}
			}
			/*
			timerState = kickTimar->Get();
			mediumButton = manipStick->GetRawButton(4);
			if (mediumButton && (lastMedium == false) && !lockout) 
			{
				mediumKicking = true;
				kickTimar->Start();
				kicking = true;
				lockout = true;
			}
			lastMedium = mediumButton;
			
			if (mediumKicking)
			{				
				if (timerState < KICKERREADYINGTIME && !kickerReadying)
				{
					RelaxKicker();
					kickerReadying = true;
				}
				if (timerState >= KICKERREADYINGTIME && !latchOpened)
				{
					OpenLatch();
					latchOpened = true;
				}
				if (timerState >= (KICKERREADYINGTIME + LATCHOPENTIME) && kickerReadying)
				{
					RetractKicker();
					kickerReadying = false;
				}
				if (timerState >= (KICKERREADYINGTIME + LATCHOPENTIME + KICKERRETRACTTIME) && latchOpened)
				{
					CloseLatch();
					latchOpened = false;
				}
				if (timerState >= (KICKERREADYINGTIME + LATCHOPENTIME + KICKERRETRACTTIME + LATCHCLOSETIME))
				{
					kickTimar->Stop();
					kickTimar->Reset();
					mediumKicking = false;
				}
			}
			
			timerState = kickTimar->Get();
			weakButton = manipStick->GetRawButton(2);
			if (weakButton && (lastWeak == false) && !lockout) 
			{
				weakKicking = true;
				kickTimar->Start();
				kicking = true;
				lockout = true;
			}	
			lastWeak = weakButton;
			if (weakKicking)
			{				
				if (timerState < KICKERREADYINGTIME && !kickerReadying)
				{
					RetractKicker();
					kickerReadying = true;
				}
				if (timerState >= KICKERREADYINGTIME && !latchOpened)
				{
					OpenLatch();
					latchOpened = true;
				}
				if (timerState >= (KICKERREADYINGTIME + LATCHOPENTIME + KICKERRETRACTTIME) && latchOpened)
				{
					CloseLatch();
					latchOpened = false;
				}
				if (timerState >= (KICKERREADYINGTIME + LATCHOPENTIME + KICKERRETRACTTIME + LATCHCLOSETIME))
				{
					kickTimar->Stop();
					kickTimar->Reset();
					weakKicking = false;
				}
			}
			*/
			
			/*strongButton = leftStick->GetTrigger();
						if (strongButton && (lastStrong == false) && !lockout) 
						{
							OpenLatch();
							Wait(1);
							ChargeKicker();
							Wait(1);
							RetractKicker();
							Wait(1);
							CloseLatch();
							Wait(1);
						}
						lastStrong = strongButton;*/
			GetWatchdog().Feed();
		}
	}
